// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ManipulatorStatusToCSVFileAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation
{
    using System;
    using System.Diagnostics;
    using System.Runtime.Serialization;
    using System.Text;

    using Microsoft.Robotics.Hardware;
    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Writes recieved ManipulatorStatusMessage messages to specified CSV file for
    /// analysis in external tools.
    /// </summary>
    [DataContract]
    [System.Diagnostics.CodeAnalysis.SuppressMessage(
    "Microsoft.Design",
    "CA1001:TypesThatOwnDisposableFieldsShouldBeDisposable",
    Justification = "Agents clean up resources when OnDeactivated() is called")]
    public class ManipulatorStatusToCSVFileAgent : ConsumerAgent<ManipulatorStatusMessage>
    {
        /// <summary>
        /// Name of the CSV file to write data to.
        /// </summary>
        [DataMember(Name = "FileName")]
        private string fileName;

        /// <summary>
        /// File stream to write to.
        /// </summary>
        private System.IO.StreamWriter file = null;

        /// <summary>
        /// Used to write column headers at beginning of file;
        /// </summary>
        private bool firstMessageReceived = false;

        /// <summary>
        /// Initializes a new instance of the <c>ManipulatorStatustoCSVFileAgent</c> class.
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="producer">ManipulatorStatusMessage producer.</param>
        /// <param name="fileName">Name of CSV file to write to.</param>
        public ManipulatorStatusToCSVFileAgent(
            string name,
            IProducer<ManipulatorStatusMessage> producer,
            string fileName)
            : base(name, producer.Name)
        {
            this.fileName = fileName;
        }

        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.file = new System.IO.StreamWriter(this.fileName, false);
        }

        public override void OnDeactivated()
        {
            base.OnDeactivated();

            if (this.file != null)
            {
                this.file.Close();
            }
        }

        /// <summary>
        /// Write to file
        /// </summary>
        /// <param name="message">A <c>ManipulatorStatusMessage</c> message.</param>
        public override void Receive(ManipulatorStatusMessage message)
        {
            string blankCsvColumn = ", ";

            if (!firstMessageReceived)
            {
                this.OutputColumnHeaders(message);
                this.firstMessageReceived = true;
            }

            StringBuilder sb = new StringBuilder();
            sb.Append(this.ToCsvValue(message.SequenceId));
            sb.Append(blankCsvColumn);
            sb.Append(this.ToCsvValue(message.State.TimeStampinMS));
            sb.Append(blankCsvColumn);
            sb.Append(this.ArrayToCsvString(message.State.CommandedJointPositions));
            sb.Append(blankCsvColumn);
            sb.Append(this.ArrayToCsvString(message.State.JointPositions));
            sb.Append(blankCsvColumn);
            sb.Append(this.ArrayToCsvString(message.State.JointTorque));       
            sb.Append(blankCsvColumn);
            sb.Append(this.ArrayToCsvString(message.State.JointVelocities));       
            sb.Append(blankCsvColumn);
            sb.Append(this.ArrayToCsvString(message.State.ExternalJointTorque));       
            sb.Append(blankCsvColumn);
            sb.Append(this.ArrayToCsvString(message.State.TcpCartesianPosition));
            sb.Append(blankCsvColumn);
            sb.Append(this.ArrayToCsvString(message.State.TcpCommandedCartesianPosition));
            sb.Append(blankCsvColumn);
            sb.Append(this.ArrayToCsvString(message.State.TcpForceTorque));
            sb.Append(blankCsvColumn);

            file.WriteLine(sb.ToString());
        }

        /// <summary>
        /// Write column headers at top of csv file;
        /// </summary>
        /// <param name="message"></param>
        private void OutputColumnHeaders(ManipulatorStatusMessage message)
        {
            StringBuilder sb = new StringBuilder();

            sb.Append("SequenceId, ");
            sb.Append(GetBlankColumns(1));
            sb.Append("TimeStampinMS, ");
            sb.Append(GetBlankColumns(1));
            sb.Append("ArmCommandedJointPositions, ");
            sb.Append(GetBlankColumns(message.State.CommandedJointPositions.Length));
            sb.Append("ArmJointPositions, ");
            sb.Append(GetBlankColumns(message.State.JointPositions.Length));
            sb.Append("ArmJointTorque, ");       
            sb.Append(GetBlankColumns(message.State.JointTorque.Length));
            sb.Append("ArmJointVelocities, ");       
            sb.Append(GetBlankColumns(message.State.JointVelocities.Length));
            sb.Append("ExternalJointTorque, ");       
            sb.Append(GetBlankColumns(message.State.ExternalJointTorque.Length));
            sb.Append("TcpCartesianPosition, ");
            sb.Append(GetBlankColumns(7));
            sb.Append("TcpCommandedCartesianPosition, ");
            sb.Append(GetBlankColumns(7));
            sb.Append("TcpForceTorque, ");
            sb.Append(GetBlankColumns(message.State.TcpForceTorque.Length));

            file.WriteLine(sb);
        }

        /// <summary>
        /// Return a string containing a number of blank csv columns
        /// </summary>
        /// <param name="numColumns">number of blank csv columns to get.</param>
        /// <returns></returns>
        private string GetBlankColumns(int numColumns)
        {
            StringBuilder sb = new StringBuilder();

            for (int i = 0; i < numColumns; ++i)
            {
                sb.Append(", ");
            }

            return sb.ToString();
        }

        /// <summary>
        /// Returns value as a string, with a trailing comma and space.
        /// </summary>
        /// <param name="value">A value.</param>
        /// <returns>A string.</returns>
        private string ToCsvValue(double value)
        {
            return value.ToString() + ", ";
        }

        /// <summary>
        /// Returns value as a string, with a trailing comma and space.
        /// </summary>
        /// <param name="value">A value.</param>
        /// <returns>A string.</returns>
        private string ToCsvValue(bool value)
        {
            if (value)
            {
                return "1, ";
            }
            
            return "0, ";
        }

        /// <summary>
        /// Generate a CSV string from an array of doubles.
        /// </summary>
        /// <param name="array">An array of doubles.</param>
        /// <returns>Csv string.</returns>
        private string ArrayToCsvString(double[] array)
        {
            StringBuilder sb = new StringBuilder();

            for (int i = 0; i < array.Length; ++i)
            {
                sb.Append(array[i].ToString());
                sb.Append(", ");
            }

            return sb.ToString();
        }

        /// <summary>
        /// Generate a CSV string from a Pose
        /// </summary>
        /// <param name="array">A pose </param>
        /// <returns>Csv string.</returns>
        private string ArrayToCsvString(Pose pose)
        {
            StringBuilder sb = new StringBuilder();

            sb.Append(pose.Position.X);
            sb.Append(", ");
            sb.Append(pose.Position.Y);
            sb.Append(", ");
            sb.Append(pose.Position.Z);
            sb.Append(", ");
            sb.Append(pose.Orientation.W);
            sb.Append(", ");
            sb.Append(pose.Orientation.X);
            sb.Append(", ");
            sb.Append(pose.Orientation.Y);
            sb.Append(", ");
            sb.Append(pose.Orientation.Z);
            sb.Append(", ");

            return sb.ToString();
        }
    }
}
